#!/usr/bin/env python3
# -*- coding:utf-8 -*-      

import rospy 
import numpy as np
import datetime
from serial_ground.msg import Cmd_send, Uav_state, FWstates, Leaderstates

STATE_UAV = 101
CMD_UAV = 102

STATE_UAV = 101
CMD_UAV = 102
ARM = 1
TAKEOFF = 2
LAND = 3
POS = 4
VEL = 5
ACC = 6
POS_VEL = 7
POS_ACC = 8
VEL_ACC = 9
POS_VEL_ACC = 10
ATT = 11


outdoor_uav_state = Uav_state()
outdoor_uav_cmd = Cmd_send()

leader_id = "plane_0"
fw_id = "plane_1"

fixed_wing_state = FWstates()

def outdoor_state_cb(msg:Uav_state):
    global outdoor_uav_state #put the value to the global varible
    outdoor_uav_state = msg
    
def state_out():
	global fixed_wing_state

	try:
		# Try to publish /plane_1/fixed_wing_control/fw_state",
		fixed_wing_state.ned_pos_x = outdoor_uav_state.local_x
		fixed_wing_state.ned_pos_y = outdoor_uav_state.local_y
		fixed_wing_state.ned_pos_z = outdoor_uav_state.local_z
		fixed_wing_state.ned_vel_x = outdoor_uav_state.local_lin_x
		fixed_wing_state.ned_vel_y = outdoor_uav_state.local_lin_y
		fixed_wing_state.ned_vel_z = outdoor_uav_state.local_lin_z
		fixed_wing_state_pub.publish(fixed_wing_state)
  
	except Exception as e:
		print(e)

###########################################################################################
# main process
if __name__ == '__main__':
	rospy.init_node('PX4_fixedwing_control_center', anonymous=True)
	rospy.loginfo("GCS node has been started!")
	rate = rospy.Rate(10)
 
	outdoor_state_sub = rospy.Subscriber("/outdoor/uav/state", Uav_state, outdoor_state_cb, queue_size=1)
 
	fixed_wing_state_pub = rospy.Publisher(fw_id + "/fixed_wing_control/fw_state", FWstates, queue_size=1)
 
	while not rospy.is_shutdown():
		state_out()
		rate.sleep()
     
	rospy.spin() #ensure the callback function can refresh immediately when the topic has a new message.
